package MULE.Bewegungsmodul;

import MULE.Bewegungsmodul.util.EngineParameter;
import MULE.Bewegungsmodul.PIDMotor;
import lejos.nxt.Motor;

/**
 * Steuerungsebene I: die Klasse Bewegung steuert die Bewegungsmotoren. Die
 * einfachsten Motorsteuerungen werden hier implementiert.
 */
public class Engine {
	// Variablen NXTRegulated
	public PIDMotor MotorLeft = new PIDMotor(Motor.A);
	public PIDMotor MotorRight = new PIDMotor(Motor.B);
	private boolean inverted;
	private boolean sync;
	//private EngineParameter driveParams;

	// Methoden
	public Engine(EngineParameter driveParams) {
		init(driveParams);
	}

	/**
	 * init ordnet, nach der Erzeugung diese Objekts durch den Server, dem
	 * Objekt einen Zugriff auf die Hardware zu.
	 */
	public void init(EngineParameter driveParams) {
		/**
		if (this.driveParams.getMotorIdLeft() == 'A') {
			MotorLeft = Motor.A;
		} else if (this.driveParams.getMotorIdLeft() == 'B') {
			MotorLeft = Motor.B;
		}

		if (this.driveParams.getMotorIdRight() == 'B') {
			MotorRight = Motor.B;
		} else if (this.driveParams.getMotorIdRight() == 'C') {
			MotorRight = Motor.C;
		}
		**/
		// Programmteil Synchronisation
		this.inverted = false;
		setSync(false);
		
		/**
		} catch (NullPointerException e) {
			System.out.println("NullPointerException: Engine.init");
		}
		**/
	}

	/**
	 * setSync erzeugt ein synchrones Verhalten der Motoren
	 * @param paramBoolean true setzt sync on, false setzt sync off
	 */
	public void setSync(boolean paramBoolean) {
		if (this.sync != paramBoolean) {
			this.sync = paramBoolean;
			if (paramBoolean) {
				this.setSpeed(this.getSpeedLeft(), this.getSpeedLeft());
			}
		}
	}

	public void invertMotors() {
		this.inverted = (!this.inverted);
	}

	public void setSpeed(double speed){
		MotorLeft.setSpeed((float)speed);
		MotorRight.setSpeed((float)speed);
	}
	
	/**
	 * setSpeed sendet Radgeschwindigkeiten an die Antriebsmotoren.
	 * 
	 * @param speed [Grad pro Sekunde]
	 * 
	 */
	public void setSpeed(double speedLeft, double speedRight) {
		if(this.sync){
			this.setSpeed(speedLeft);
		} else {
			MotorLeft.setSpeed((float)speedLeft);
			MotorRight.setSpeed((float)speedRight);
		}

	}

	/**
	 *@return Grad pro Sekunde des linken Motors
	 */
	public double getSpeedLeft() {
		double result = MotorLeft.getSpeed();
		return result;
	}

	/**
	 *@return Grad pro Sekunde des rechten Motors
	 */
	public double getSpeedRight() {
		double result = MotorRight.getSpeed();
		return result;
	}

	/**
	 * Linken Motor bewegen
	 */
	public void forwardMotorLeft() {
		setSync(false);
		if (this.inverted) {
			MotorLeft.backward();
		} else {
			MotorLeft.forward();
		}
	}

	/**
	 *Rechten Motor bewegen
	 */
	public void forwardMotorRight() {
		setSync(false);
		if (this.inverted) {
			this.MotorRight.backward();
		} else {
			this.MotorRight.forward();
		}
	}

	/**
	 *
	 */
	public void backwardMotorLeft() {
		setSync(false);
		if (this.inverted) {
			MotorLeft.forward();
		} else {
			MotorLeft.backward();
		}
	}

	/**
	 *
	 */
	public void backwardMotorRight() {
		setSync(false);
		if (this.inverted) {
			MotorRight.forward();
		} else {
			MotorRight.backward();
		}
	}

	/**
	 *
	 */
	public void stopMotorLeft() {
		setSync(false);
		MotorLeft.stop();
	}

	/**
	 *
	 */
	public void stopMotorRight() {
		setSync(false);
		MotorRight.stop();
	}

	/**
	 *
	 */
	public void stop() {
		this.stopMotorLeft();
		this.stopMotorRight();
	}

	/**
	 *
	 */
	public void forward() {
		setSync(true);

		if (this.inverted) {
			this.MotorLeft.backward();
			this.MotorRight.backward();
		} else {
			this.MotorLeft.forward();
			this.MotorRight.forward();
		}
	}

	/**
	 *
	 */
	public void backward() {
		setSync(true);

		if (this.inverted) {
			this.MotorLeft.forward();
			this.MotorRight.forward();
		} else {
			this.MotorLeft.backward();
			this.MotorRight.backward();
		}
	}
}
